/**
 * @file serial.cpp
 * @author circleup (circleup@foxmial.com)
 * @brief serial 接受节点
 * @version 0.1
 * @date 2020-07-13
 * 
 * @copyright Copyright (c) 2020
 * 
 */

#include "serial_driver.h"

#include <iostream>
#include <string>

/**
 * @brief 串口数据处理节点
 * 
 * @param argc 
 * @param argv 
 * @return int 
 */
int main(int argc, char** argv)
{
  ros::init(argc, argv, "ROS_serial");

  ros::NodeHandle serial;

  ros::Publisher send = serial.advertise<storm_car::car_status>("serial_receive_data", 1000);
  
  ros::Rate loop_rate(10);

  storm_car::car_status msg;

  serial::Serial sp;
  serial::Timeout to = serial::Timeout::simpleTimeout(100);
  sp.setPort("/dev/ttyUSB0");
  sp.setBaudrate(115200);
  sp.setTimeout(to);

  try
  {
    sp.open();
  }
  catch(serial::IOException& e)
  {
    ROS_INFO("Unable to open port.");
    return -1;
  }
  
  if(sp.isOpen())
  {
    ROS_INFO("/dev/ttyUSB0 is opened.");
  }
  else
  {
    return -1;
  }

  char cmd[] = "$GPCHC,2111,269012.40,268.34,-2.14,-1.95,-0.00,-0.04,0.28,0.0339,-0.0372,0.9965,31.824154570,117.116787479,31.81,0.005,0.005,-0.025,0.007,23,19,41,1,2*5E\x0d\x0a";

  while(ros::ok())
  {
    size_t n = sp.available();
    if(n!=0)
    {
      uint8_t buffer[2048];
      uint16_t length = 0;
      n = sp.read(buffer, n);
      length = WriteToBuffer(&g_serial_buffer, (char *)&buffer, n);
      length = ReadFromBuffer((char *)&buffer, &g_serial_buffer, sizeof(buffer));
      if(0 != length)
      {
        msg = AnalysisData((char*)buffer);
        send.publish(msg);
      }
    }

    // sp.write((uint8_t *)cmd,sizeof(cmd));

    loop_rate.sleep();
  }
  
  //关闭串口
  sp.close();

  return 0;
}